package org.pewpew.server;

public class GeoMath {
	private static final double EPSILON = 0.000000000001;
	private static final int EARTH_RADIUS = 6366000;
	private static final double RADIAN_TO_DEGREE = 180/Math.PI;

	public static double distance(double latitudeA, double longitudeA, double latitudeB, double longitudeB) {
	    double a1 = latitudeA / RADIAN_TO_DEGREE;
	    double a2 = longitudeA / RADIAN_TO_DEGREE;
	    double b1 = latitudeB / RADIAN_TO_DEGREE;
	    double b2 = longitudeB / RADIAN_TO_DEGREE;

	    double t1 = Math.cos(a1)*Math.cos(a2)*Math.cos(b1)*Math.cos(b2);
	    double t2 = Math.cos(a1)*Math.sin(a2)*Math.cos(b1)*Math.sin(b2);
	    double t3 = Math.sin(a1)*Math.sin(b1);
	    double tt = Math.acos(t1 + t2 + t3);
	   
	    return EARTH_RADIUS*tt;
	}
	
	public static double azimuth (double longitudeDifference, double latitudeDifference) {
		
	    double azimuth = (Math.PI / 2) - Math.atan(latitudeDifference / longitudeDifference);
	    double result;
		if (longitudeDifference > 0) {
	    	result = azimuth;
	    }
	    else if (longitudeDifference < 0) {
	    	result = azimuth + Math.PI;
	    }
	    else if (latitudeDifference >= 0 && Math.abs(longitudeDifference) < EPSILON) {
	    	result = 0;
	    }
	    else if (latitudeDifference < 0 && Math.abs(longitudeDifference) < EPSILON) {
	    	result = Math.abs(latitudeDifference) < EPSILON ? 0.0 : Math.PI;
	    	//System.out.println("2 [" + longitudeDifference + "," + latitudeDifference + "]=>" + result);			
	    }
	    else {
	    	throw new RuntimeException("Cannot be: 3 [" + longitudeDifference + "," + latitudeDifference + "]");
	    }
	    return result * RADIAN_TO_DEGREE;
	}
	
	
}
